﻿using System;
using System.Collections.Generic;
using System.Text;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// Version of the cone model optimised for laser.
    /// Just draws lines.
    /// </summary>
    public class ConeModelLaser : IConeModel
    {
        private double _maxRange;
        private double _obstacleSize;
        private double _full= Math.Log(0.8 / 0.2);
        private double _empty = Math.Log(0.4 / 0.6);
        private TrigUtils _trig = new TrigUtils();

        public ConeModelLaser(double maxRange, double obstacleSize)
        {
            _maxRange = maxRange;
            _obstacleSize = obstacleSize;
        }
        #region IConeModel Members

        /// <summary>
        /// DEPRECIATED
        /// </summary>
        /// <param name="map"></param>
        /// <param name="state"></param>
        /// <param name="reading"></param>
        /// <param name="cmPerPixel"></param>
        /// <param name="updateRect"></param>
        /// <returns></returns>
        public double[,] UpdateMap(double[,] map, Pose2D state, double[] reading, double cmPerPixel, out System.Drawing.Rectangle updateRect)
        {
            throw new Exception("The method or operation is not implemented.");
        }

        /// <summary>
        /// DEPRECIATED ?
        /// </summary>
        /// <param name="map"></param>
        /// <param name="reading"></param>
        /// <param name="cmPerPixel"></param>
        /// <returns></returns>
        public double[,] UpdateMap(double[,] map, DistanceReading reading, double cmPerPixel)
        {
            throw new Exception("The method or operation is not implemented.");
        }

        public void UpdateMap(ref double[,] map, DistanceReading reading, double cmPerPixel)
        {
            double readingTheta = _trig.CleanAngle(reading.Theta);
            int mapWidth = map.GetLength(0);
            int mapHeight = map.GetLength(1);
            double oneOverCmPerPixel = 1.0 / cmPerPixel;
            int moX = (int)Math.Round(reading.X * oneOverCmPerPixel);
            int moY = (int)Math.Round(reading.Y * oneOverCmPerPixel);
            int scaledDistance = (int)Math.Round(reading.Distance * oneOverCmPerPixel);
            double obstacleEnd = reading.Distance + ObstacleSize;

            double halfScaledObstacleSize = _obstacleSize / (2 * cmPerPixel);
            double modelRange = scaledDistance + halfScaledObstacleSize;

            double cosTheta = Math.Cos(readingTheta);
            double sinTheta = Math.Sin(readingTheta);

            int steps = (int) Math.Round(modelRange);
            for(int i=0; i<= steps; i++)
            {
                int x = (int) Math.Round(moX + i*cosTheta);
                int y = (int) Math.Round(moY + i*sinTheta);

                // NOTE this will prevent scans that start off the map
                if (x >=0 && x < mapWidth && y>=0 && y <mapHeight)
                {
                    double realXdist = i * cosTheta * cmPerPixel;
                    double realYdist = i * sinTheta * cmPerPixel;
                    double realDistance = Math.Sqrt(realXdist * realXdist + realYdist * realYdist);
                    if (realDistance < reading.Distance)
                    {
                        // TODO use real probabilities
                        map[x, y] += _empty;
                    } else if (realDistance > obstacleEnd || realDistance >= _maxRange)
                    {
                        // all done
                        break; 
                    } else
                    {
                        map[x, y] += _full;
                    }
                } else
                {
                    break;
                }
            }
            
        }

        public double ConeWidth
        {
            get { return 0; }
        }

        public double ObstacleSize
        {
            get { return _obstacleSize; }
        }

        #endregion
    }
}
